% run_extend_return_map

clc
clear
close all
format compact
addpath('../animations', '../controllers', '../dynamics', '../environments', '../library');

% x_dot_vec = [0:0.1:2.0
%
% for iter = 1:length(x_dot_vec)

y_pert_vec = [-0.09:0.005:0.09];

x_dot = 0;


y_fp = 1.0;

y = y_fp;

uniform_damping = 1000e3;

first_guess = (4*pi/8);

td_angle = fc_fixed_point_solve;

[time_passive, force_passive, output] = passive_force_profile;

trigger_velocity = min(output{1,2}(:,ForceControlModelClass.dyci));




for iter = 1:numel(y_pert_vec)
    
    
    y = y_fp + y_pert_vec(iter);
    
    %     try
    %         [time_fc, force_fc, output_fc, J_fc, controller_fc] = run_force_control(time_passive, force_passive);
    %     catch
    
    %     end
    
    % J_opt
    % J_fc
    
    % close all
    % plot(time_passive, force_passive, 'k--', time_opt, force_opt, 'r-', time_fc, force_fc, 'b-')
    
    % close all
    % plot(time_passive, force_passive, 'k--', time_fc, force_fc, 'b-')
    
    % figure
    % plot(output_fc{1,1}(:,1), output_fc{1,2}(:,4))
    
    % dxout_fc = output_fc{3,2}(end,ForceControlModelClass.dxci)
    % yout_fc = output_fc{3,2}(end,ForceControlModelClass.yci)
    %
    % dxout_fc_error = output_fc{3,2}(end,ForceControlModelClass.dxci) - x_dot
    % yout_fc_error = output_fc{3,2}(end,ForceControlModelClass.yci) - y
    
    %
    % max_torque = controller_fc.max_torque
    %
    % T_in = 0.5*ForceControlModelClass.m*x_dot^2
    % V_in = ForceControlModelClass.m*ForceControlModelClass.g*y
    %
    % T_out = 0.5*ForceControlModelClass.m*dxout_fc^2
    % V_out = ForceControlModelClass.m*ForceControlModelClass.g*yout_fc
    %
    % E_net = T_out + V_out - T_in - V_in
    %
    % E_adjust = J_fc-E_net
    
    
    
    [time, force, output, J, controller] = run_force_control_extend(time_passive, force_passive, trigger_velocity);
    
    % close all
    % plot(time_passive, force_passive, 'k--', time, force, 'b-')
    % pause(0.05)
    
    % dxout_fc_error = output{3,2}(end,ForceControlModelClass.dxci) - x_dot
    y_pert = y_pert_vec(iter)
%     yout_fc_error = output_fc{3,2}(end,ForceControlModelClass.yci) - y
    yout_extend_error = output{3,2}(end,ForceControlModelClass.yci) - y
    
    %     return_map(iter, 1:3) = [y, output_fc{3,2}(end,ForceControlModelClass.yci), output{3,2}(end,ForceControlModelClass.yci)];
    return_map(iter, 1:2) = [y, output{3,2}(end,ForceControlModelClass.yci)];
    
    plot(return_map(:,1), return_map(:,2), 'b-')
    
end

